////
////  test ceres.cpp
////  markerARLib
////
////  Created by fg on 5/16/16.
////  Copyright © 2016 CvidKal. All rights reserved.
////
//
//
//
//
//
//
//
//#include "ceres/ceres.h"
//#include "glog/logging.h"
//#include <ceres/rotation.h>
//#include "Frame.hpp"
//#include <sophus/se3.h>
//#include "PoseOptimiser.hpp"
//using ceres::AutoDiffCostFunction;
//using ceres::CostFunction;
//using ceres::Problem;
//using ceres::Solver;
//using ceres::Solve;
//
//using ceres::SizedCostFunction;
//using namespace markerAR;
//
//class BundlerResidual{
//public:
//    BundlerResidual(double u,double v,double*pointXYZ):u(u),v(v),pointXYZ(pointXYZ){}
//    template<typename T>
//    bool operator()(const T*const camera,
//                    T* residual
//                    )const
//    {
//        T p[3];
//        T point[3];
//        point[0] = T(pointXYZ[0]);
//        point[1] = T(pointXYZ[1]);
//        point[2] = T(pointXYZ[2]);
//        ceres::AngleAxisRotatePoint(camera, point, p);
//        //apply translation
//        p[0]+=camera[3];
//        p[1]+=camera[4];
//        p[2]+=camera[5];
//        
//        const T& focal = camera[6];
//        
//        T xp =  focal * p[0] / p[2];
//        T yp =  focal * p[1] / p[2];
//        
//        residual[0] = xp-u;
//        residual[1] = yp-v;
//        return  true;
//    }
//    
//private:
//    double u,v;
//    double * pointXYZ;
//};
//
////
//void BuildProblem(Frame*pf)
//{
//
//}
//int main(int argc,char**argv)
//{
//    google::InitGoogleLogging(argv[0]);
//
//        std::vector<Feature*> fts;
//        std::srand(std::time(0)); //use current time as seed for random generator
//    
//    
//        Eigen::Matrix3d k;
//        double fx=420;
//        k(0,0) = k(1,1) = fx;
//        k(0,2) = 640/2;
//        k(1,2) = 480/2;
//        IntrinsicMatrix K(k);
//        Frame frame;
//        frame.K = &K;
//        Pose p1;
//        p1.R.setIdentity();
//        p1.t.setZero();
//        p1.t[2] = 0.2;
//        frame.C = p1;
//        Sophus::SE3 pose(p1.R,p1.t);
//        auto invPose = pose.inverse();
//        double depth = 500;
//        for (int i=0; i<300; i++) {
//            float x = (rand()/(float)RAND_MAX)*640;
//            float y =(rand()/(float)RAND_MAX)*480;
//    
//            Feature * f = new Feature(x,y,&frame);
//            
//            frame.fts.push_back(f);
//            auto worldP = invPose*(f->f*depth);
//            auto p2 = pose*worldP;
//            MapPoint * mp = new MapPoint(worldP);
//            f->pointXYZ = mp;
//        }
//    frame.C.t[2] = 0;
//    PoseOptimiser::PoseOptimiseByCeres(&frame);
//
//    return 0;
//
//}